#include "skywatcher.h"
#include "usart.h"

/* ----------------------------------------------------------------------- */
/* Bits de configuration: adapté à votre dispositif et à vos besoins */

static __code char __at (__CONFIG1H) CONFIG1H = _OSC_HSPLL_1H & _FCMEN_ON_1H & _IESO_ON_1H;
static __code char __at (__CONFIG2H) CONFIG2H = _WDTEN_OFF_2H & _WDPS_32768_2H & _WINEN_OFF_2H;
/*code char at __CONFIG1H CONFIG1H = _OSC_HSPLL_1H & _FCMEN_ON_1H & _IESO_ON_1H;
code char at __CONFIG2L CONFIG2L = _PWRTEN_ON_2L & _BOREN_ON_2L & _BORV_20_2L;
code char at __CONFIG2H CONFIG2H = _WDTEN_OFF_2H & _WDPS_32768_2H & _WINEN_OFF_2H;
code char at __CONFIG3L CONFIG3L = _PWMPIN_OFF_3L & _LPOL_HIGH_3L & _HPOL_HIGH_3L & _T1OSCMX_ON_3L;
code char at __CONFIG3H CONFIG3H = _FLTAMX_RC1_3H & _SSPMX_RC7_3H & _PWM4MX_RB5_3H & _EXCLKMX_RC3_3H & _MCLRE_ON_3H;
code char at __CONFIG4L CONFIG4L = _STVREN_ON_4L & _LVP_ON_4L & _DEBUG_OFF_4L;
code char at __CONFIG5L CONFIG5L = _CP0_OFF_5L & _CP1_OFF_5L & _CP2_OFF_5L & _CP3_OFF_5L;
code char at __CONFIG5H CONFIG5H = _CPB_OFF_5H & _CPD_OFF_5H;
code char at __CONFIG6L CONFIG6L = _WRT0_OFF_6L & _WRT1_OFF_6L & _WRT2_OFF_6L & _WRT3_OFF_6L;
code char at __CONFIG6H CONFIG6H = _WRTC_OFF_6H & _WRTB_OFF_6H & _WRTD_OFF_6H;
code char at __CONFIG7L CONFIG7L = _EBTR0_OFF_7L & _EBTR1_OFF_7L & _EBTR2_OFF_7L & _EBTR3_OFF_7L;
code char at __CONFIG7H CONFIG7H = _EBTRB_OFF_7H;
*/
//resolution 9 bits PWMFREQ=78125hz@40Mhz 
//unsigned int pwm_table[] = {0x0000, 0x0071, 0x00de, 0x013f, 0x0190, 0x01cd, 0x01f3, 0x01ff}; 
unsigned int pwm_table[] = {0x0000, 0x0035, 0x006a, 0x009e, 0x00d0, 0x00ff, 0x012c, 0x0156, 0x017c, 0x019e, 0x01bb, 0x01d3, 0x01e6, 0x01f4, 0x01fd, 0x01ff}; 

#define PWMFULL 0x1ff
/* PWMPERIOD=12800ns i.e. 128xTIMERPERIOD: for low speeds, step_delay should be at least 16xPWMPERIOD */

/*
unsigned long ra_position = 0x800000;
unsigned char ra_microstep = 0x00;
unsigned char ra_pwmindex = MICROSTEPS - 1;
unsigned long ra_wormperiod = 0x256;
unsigned long ra_target = 0x000001; 
unsigned long ra_target_current = 0x000000;
unsigned long ra_target_slow = 400;
unsigned long ra_breaks = 400;
TimerValue ra_timer;
speedRampData ra_speedramp;
*/
#define ra motors[0]
/*
unsigned long de_position = 0x800000;
unsigned char de_microstep = 0x00;
unsigned char de_pwmindex = MICROSTEPS - 1;
unsigned long de_wormperiod = 0x256;
unsigned long de_target = 0x800000;
unsigned long de_target_current = 0x000000;
unsigned long de_target_slow = 400;
unsigned long de_breaks = 200;
TimerValue de_timer;
speedRampData de_speedramp;
*/
#define de motors[1]
/* Motor status (Skywatcher protocol) */
/*
unsigned int ra_status=0X0010; // lowspeed, forward, slew mode, stopped
unsigned int de_status=0X0010; // lowspeed, forward, slew mode, stopped
*/
volatile unsigned char compute_next_ra;
volatile unsigned char compute_next_de;

/** Initialisations **/

void init_pwm(void) {
  PTCON0 = 0x00; // postscaler = 1:1 (0000); prescaler= 1:1 (00); Free running mode (00)
  PTCON1 = 0x00; // PTMR off (0); PTMR direction upwards (0); unused (000000)
  // PWMCON0 = 0x6F; // unused (0); pwm1&3 enabled (0110); all independent pairs (1111)
  PWMCON0 = 0x7F; // unused (0); pwm1&3&5&7 enabled (0111); all independent pairs (1111)
  PWMCON1 = 0x00; //??
  //    PTPERL = 0xff; // Fpwm = FOSC / (4 * (PTPER + 1))= 19531.25 Hz
  //    PTPERL = 0x3f; // Fpwm = 78125Hz@20Mhz 8bits resolution, 156250Hz@40Mhz
  PTPERL = 0x7f; // Fpwm = 78125Hz@40Mhz 9bits resolution
  //    PTPERL = 0x1f; // Fpwm = 156250Hz
  PTPERH = 0x00;

  //Start PWM
  PTCON1bits.PTEN=1;
}

void init_timers(void) {
  // Init TMR1
  T1CON = 0x04; // 2x8bits mode (RD16=0), TMR1RUN is read only (0), prescale = 1 (00), 
                // use internal clock (000), don't run yet (TMR1ON = 0)  
  // Init TMR0
  T0CON = 0x08; // 2x8bits mode (T016=0), no prescaler (PSA=1), prescale = xxx (xxx), 
                // use internal clock (TOCS=0), don't run yet (TMR0ON = 0)  
}

#define AXIS(index) motors[index]

/* Motor Start/Stop */

void compute_timer(unsigned char axisindex) {
  unsigned long mul_period, rem_period, steps_worm, n, period;
  if (axisindex == 0) {
    mul_period=MUL_RA;
    rem_period=REM_RA;
    steps_worm=RA_STEPS_WORM;
  } else {
    mul_period=MUL_DE;
    rem_period=REM_DE;
    steps_worm=DE_STEPS_WORM;
  }

  if (GETMOTORPROPERTY(AXIS(axisindex).status, HIGHSPEED) && (AXIS(axisindex).wormperiod < (MICROSTEPS * AXIS(axisindex).minperiod))) 
    period = MICROSTEPS * AXIS(axisindex).minperiod;
  else
    if (AXIS(axisindex).wormperiod < AXIS(axisindex).minperiod) 
      period = AXIS(axisindex).minperiod;
    else 
      period = AXIS(axisindex).wormperiod;
  n = (period * mul_period) / PRECISION ;
  n += (period * rem_period) / (steps_worm * PRECISION);
  /*
  n = (AXIS(axisindex).wormperiod * mul_period) ;
  n += ((AXIS(axisindex).wormperiod * rem_period) / steps_worm);
  */

  n = n * TICKS_MICRO;

  if (n == 0) { AXIS(axisindex).timer.count=1; AXIS(axisindex).timer.timer.value=MIN_TICKS; }
  else if (n > 65535) { AXIS(axisindex).timer.count = (n / 65536) + 1; AXIS(axisindex).timer.timer.value = 65536 - (n / AXIS(axisindex).timer.count); }
  else {  AXIS(axisindex).timer.count = 1; if (n < MIN_TICKS) n = MIN_TICKS;  n = 65536 - n; AXIS(axisindex).timer.timer.value = n;}

}

void resume(unsigned char axisindex) {
  compute_timer(axisindex);
  AXIS(axisindex).timer.current=0;
 
  // Acceleration phase
  //if (ra_timer.count > 1) { ra_timer.count=1; ra_timer.value=0; } // lowest possible high speed
  AXIS(axisindex).speedramp.min_delay= (65536 - AXIS(axisindex).timer.timer.value) * AXIS(axisindex).timer.count;
  AXIS(axisindex).speedramp.step_delay = C0_APPROX;
  AXIS(axisindex).timer.count=C0_TIMER_COUNT;
  AXIS(axisindex).timer.timer.fields.valueh=C0_TIMER_VALUEH;
  AXIS(axisindex).timer.timer.fields.valuel=C0_TIMER_VALUEL;
  SETTIMER(axisindex);
  AXIS(axisindex).speedramp.rest=0;
  AXIS(axisindex).speedramp.accel_count=0;
  AXIS(axisindex).speedramp.run_state = ACCEL;
  
  if GETMOTORPROPERTY(AXIS(axisindex).status, HIGHSPEED) {
      SETPWMA(axisindex, PWMFULL);
      SETPWMB(axisindex, PWMFULL);
    } else {
      AXIS(axisindex).timer.count=1;
      AXIS(axisindex).speedramp.step_delay = C0_APPROX / 8;
  }
  if (!(GETMOTORPROPERTY(AXIS(axisindex).status, SLEWMODE))) AXIS(axisindex).target_current = 0;
  if (axisindex==0) compute_next_ra = 0; else compute_next_de=0;
  SETMOTORPROPERTY(AXIS(axisindex).status, RUNNING);
  STARTTIMER(axisindex);
}


void pause(unsigned char axisindex) {
  if ((!(GETMOTORPROPERTY(AXIS(axisindex).status, HIGHSPEED))) && (AXIS(axisindex).wormperiod > 19)) {
    STOPTIMER(axisindex);
    UNSETMOTORPROPERTY(AXIS(axisindex).status, RUNNING);
     if (axisindex==0) compute_next_ra = 0; else compute_next_de=0;
  } else { 
    AXIS(axisindex).speedramp.accel_count = 0;
    AXIS(axisindex).speedramp.rest = 0;
    AXIS(axisindex).speedramp.decel_start = AXIS(axisindex).breaks;
    AXIS(axisindex).speedramp.run_state = DECEL;
  }
  
}

void stop(unsigned char axisindex) {
  STOPTIMER(axisindex);
  UNSETMOTORPROPERTY(AXIS(axisindex).status, RUNNING);
}

void compute_next(unsigned char axisindex) {
  unsigned char step_inc;
  unsigned char remember_tx=PIE1bits.TXIE;
  PIE1bits.TXIE=0;

  if  ((!(GETMOTORPROPERTY(AXIS(axisindex).status, SLEWMODE))) && (AXIS(axisindex).target_current >= AXIS(axisindex).target - AXIS(axisindex).target_slow) && (AXIS(axisindex).speedramp.run_state != DECEL)) {
      AXIS(axisindex).speedramp.accel_count = 0;
      AXIS(axisindex).speedramp.rest = 0;
      if (GETMOTORPROPERTY(AXIS(axisindex).status, HIGHSPEED))
	AXIS(axisindex).speedramp.decel_start = (AXIS(axisindex).target_slow / 8);
      else
	AXIS(axisindex).speedramp.decel_start = AXIS(axisindex).target_slow;
      AXIS(axisindex).speedramp.run_state = DECEL;
  }
  if (AXIS(axisindex).speedramp.run_state == ACCEL) {
      AXIS(axisindex).speedramp.accel_count += 1;
      AXIS(axisindex).speedramp.step_delay=AXIS(axisindex).speedramp.step_delay - (((2 * AXIS(axisindex).speedramp.step_delay) + AXIS(axisindex).speedramp.rest) / (4 * AXIS(axisindex).speedramp.accel_count + 1));
      AXIS(axisindex).speedramp.rest = ((2 * AXIS(axisindex).speedramp.step_delay)+AXIS(axisindex).speedramp.rest)%(4 * AXIS(axisindex).speedramp.accel_count + 1);
      if (AXIS(axisindex).speedramp.step_delay <= AXIS(axisindex).speedramp.min_delay) {
	AXIS(axisindex).speedramp.step_delay = AXIS(axisindex).speedramp.min_delay;
	AXIS(axisindex).speedramp.run_state = RUN;
      }
      if (AXIS(axisindex).speedramp.step_delay > 65535) { 
	AXIS(axisindex).timer.count = (AXIS(axisindex).speedramp.step_delay / 65536) + 1; 
	AXIS(axisindex).timer.timer.value = 65536 - (AXIS(axisindex).speedramp.step_delay / AXIS(axisindex).timer.count); 
      } else {  
	AXIS(axisindex).timer.count = 1; 
	//if (n > MIN_TICKS) n = MIN_TICKS; 
	AXIS(axisindex).timer.timer.value = 65536 - AXIS(axisindex).speedramp.step_delay;
      }
      //SETTIMER(axisindex);  
  }
  
  if (AXIS(axisindex).speedramp.run_state == DECEL) {
       AXIS(axisindex).speedramp.accel_count += 1;
       if (AXIS(axisindex).speedramp.accel_count == AXIS(axisindex).speedramp.decel_start) {
	 // STOPTIMER(axisindex);
	 //UNSETMOTORPROPERTY(AXIS(axisindex).status, RUNNING);
       } else {
	 AXIS(axisindex).speedramp.step_delay=AXIS(axisindex).speedramp.step_delay + (((2 * AXIS(axisindex).speedramp.step_delay) + AXIS(axisindex).speedramp.rest) / (4 * (AXIS(axisindex).speedramp.decel_start - AXIS(axisindex).speedramp.accel_count) + 1));
	 AXIS(axisindex).speedramp.rest = ((2 * AXIS(axisindex).speedramp.step_delay)+AXIS(axisindex).speedramp.rest)%(4 * (AXIS(axisindex).speedramp.decel_start - AXIS(axisindex).speedramp.accel_count) + 1);
	 if (AXIS(axisindex).speedramp.step_delay > 65535) { 
	   AXIS(axisindex).timer.count = (AXIS(axisindex).speedramp.step_delay / 65536) + 1; 
	   AXIS(axisindex).timer.timer.value = 65536 - (AXIS(axisindex).speedramp.step_delay / AXIS(axisindex).timer.count); 
	 } else {  
	   AXIS(axisindex).timer.count = 1; 
	   //if (n > MIN_TICKS) n = MIN_TICKS; 
	   AXIS(axisindex).timer.timer.value = 65536 - AXIS(axisindex).speedramp.step_delay;
	 }
	 // SETTIMER(axisindex);  
       }
  }
     
     if (!(GETMOTORPROPERTY(AXIS(axisindex).status, HIGHSPEED))) step_inc=1; else step_inc=MICROSTEPS;
     if (GETMOTORPROPERTY(AXIS(axisindex).status, BACKWARD)) {
       AXIS(axisindex).position -= step_inc; AXIS(axisindex).microstep -= step_inc; 
     } else {
       AXIS(axisindex).position += step_inc; AXIS(axisindex).microstep += step_inc; 
     }
     AXIS(axisindex).target_current += step_inc;
     AXIS(axisindex).microstep &= (4 * MICROSTEPS) - 1;
     if (!(GETMOTORPROPERTY(AXIS(axisindex).status, HIGHSPEED))) {
       AXIS(axisindex).pwmindex = AXIS(axisindex).microstep & MICROSTEP_MASK;
       if ((AXIS(axisindex).microstep & PWM_MASK) == 0) AXIS(axisindex).pwmindex = MICROSTEPS - 1 - AXIS(axisindex).pwmindex;
     }
     if (axisindex==0) compute_next_ra = 0; else compute_next_de=0;
     if (remember_tx) PIE1bits.TXIE = 1;
}

/*** Interrupts ***/
/** High Priority Interrupt Subroutine **/
void high_isr (void) __interrupt 1 {

  unsigned char axisindex=0;
  if (PIR1bits.TMR1IF==1) {axisindex = 0;PIR1bits.TMR1IF = 0; }
  else if (INTCONbits.TMR0IF==1) { axisindex = 1; INTCONbits.TMR0IF = 0;}
  AXIS(axisindex).timer.current += 1;
  SETTIMER(axisindex); 
  if (AXIS(axisindex).timer.current == AXIS(axisindex).timer.count) {
    AXIS(axisindex).timer.current=0;
    
     if (!(GETMOTORPROPERTY(AXIS(axisindex).status, HIGHSPEED))) {
       SETPWMA(axisindex, pwm_table[AXIS(axisindex).pwmindex]); 
       AXIS(axisindex).pwmindex = MICROSTEPS - 1 - AXIS(axisindex).pwmindex;
       SETPWMB(axisindex, pwm_table[AXIS(axisindex).pwmindex]);
     }
     SETPHIA(axisindex);
     SETPHIB(axisindex);

     if (AXIS(axisindex).speedramp.run_state == DECEL) {
       //AXIS(axisindex).speedramp.accel_count += 1;
       if (AXIS(axisindex).speedramp.accel_count == AXIS(axisindex).speedramp.decel_start) {
	 STOPTIMER(axisindex);
	 UNSETMOTORPROPERTY(AXIS(axisindex).status, RUNNING);
	 return;
       } 
     }

     if (axisindex==0) compute_next_ra = 1; else compute_next_de=1;
  }


}

/** Low Priority Interrupt Subroutine **/
void low_isr (void) __interrupt 2 { // low priority interrupt

   if (PIR1bits.RCIF) {   // Usart Receive
     unsigned char c;
     c=RCREG;
     cmd[usart_index]=c;
     usart_index+=1;
     if (usart_index==32) { send_byte(0x01); usart_index=0;}
     if (c == 0x2e) usart_index=0; // flush '.' ending commands
     if (c == 0x0D) cmd[usart_index - 1] = (c = 0);
     if (c==0) usart_rdy=1;
  }

   if (( PIR1bits.TXIF) && (PIE1bits.TXIE))  {
     TXREG = txbuffer.buffer[txbuffer.tail++];
     if (txbuffer.tail == TXSIZE) txbuffer.tail=0;
     if (txbuffer.tail == txbuffer.head) PIE1bits.TXIE=0;
   }
} 

void main() {
  unsigned char axisindex;
  /** Init interrupts **/
  /* Using priority */
  RCONbits.IPEN=1;
  /* Interrupt priority mask */
  // high priority interrupts
  IPR1bits.TMR1IP=1;
  INTCON2bits.TMR0IP=1;
  //   IPR1bits.TMR2IP=1;
  //   IPR3bits.PTIP=1;
  //   INTCON2bits.RBIP=1;
  // low priority interrupts
  IPR1bits.RCIP=0;
  IPR1bits.TXIP=0;
  /* Interrupt Mask */ 
  //   PIE1bits.RCIE=1;
  PIE1bits.TMR1IE=1;
  INTCONbits.TMR0IE=1;
  //  PIE1bits.TMR2IE=1;
  //  PIE3bits.PTIE=1;
  
  INTCONbits.GIE=1; // enable high priority ints
  INTCONbits.PEIE=1; // enable low priority ints
  
  /** Init I/O Ports **/
  RAENABLE=0;
  DEENABLE=0;
  SETRAOUTPUTPORTS;
  SETDEOUTPUTPORTS;
  SETLEDOUTPUTPORTS;
  //SETINPUTPORTS;
  
  /** Init Usart **/
  // usart_open(129); /* 10-> 115200, 32 -> 38400, 64 -> 19200 , 129 -> 9600 @20Mhz */
  usart_open(255); /* 255 -> 9600 @ 40 Mhz */
  compute_next_ra=0;
  compute_next_de=0;
  while (1) {
    if (compute_next_ra) compute_next(0);
    if (compute_next_de) compute_next(1);
    if (usart_rdy) {            // Usart cmd received
      if (cmd[0] != ':') { send_byte('!'); goto next_cmd;}
      if (cmd[2] == '1') axisindex = 0; else if (cmd[2] == '2') axisindex=1; else goto cant_do;
      switch (cmd[1]) {
	/** Syntrek Protocol **/
      case 'e': 				// Get firmware version
        send_byte('='); send_string(VERSION);
	break;
      case 'a': 				// Get number of microsteps per revolution
	if (cmd[2] == '1') {send_byte('='); send_u24(RA_STEPS_360);}
	else if (cmd[2] == '2') { send_byte('='); send_u24(DE_STEPS_360);}
	break;
      case 'b': 		               // Get number of microsteps per revolution
	if (cmd[2] == '1') {send_byte('='); send_u24(RA_STEPS_WORM * PRECISION);}
	else if (cmd[2] == '2') { send_byte('='); send_u24(DE_STEPS_WORM * PRECISION);}
	break;
      case 'D': 		               // Get Worm period
	send_byte('=');send_u24(AXIS(axisindex).wormperiod);
	break;
      case 'f':				// Get motor status
	send_byte('=');send_u12(AXIS(axisindex).status);
	break;
      case 'j': 		               // Get encoder values
	send_byte('=');send_u24(AXIS(axisindex).position);
	break;
      case 'g':				// get high speed ratio
	send_byte('=');send_u24(MICROSTEPS);
	break;
      case 'E':                                          	// Set encoder values
	AXIS(axisindex).position=get_u24();
	send_byte('=');
	break;
      case 'F':                                          	// Initialize & activate motors
	if ((cmd[2] != '1') && (cmd[2] != '2') && (cmd[2] != '3')) goto cant_do;
	init_pwm();	
	init_timers();
	if ((cmd[2] == '1') || (cmd[2] == '3')) { 
	  SETRAPWMA(pwm_table[ra.pwmindex]); SETRAPWMB(pwm_table[MICROSTEPS - 1 - ra.pwmindex]);
	  SETRAPHIA(ra.microstep); SETRAPHIB(ra.microstep);
          SETMOTORPROPERTY(ra.status, INITIALIZED);
	  RAENABLE = 1;
	}
	if ((cmd[2] == '2') || (cmd[2] == '3')) { 
	  SETDEPWMA(pwm_table[de.pwmindex]); SETDEPWMB(pwm_table[MICROSTEPS - 1 - de.pwmindex]);
	  SETDEPHIA(de.microstep); SETDEPHIB(de.microstep);
          SETMOTORPROPERTY(de.status, INITIALIZED);
	  DEENABLE = 1;
	}
	send_byte('=');
	break;

      case 'J':						// Start motor
	resume(axisindex);
	send_byte('=');
	break;
      case 'K':					// Stop motor
	pause(axisindex);
  	send_byte('=');
	break;
      case 'L':  						// Instant Stop motor
	stop(axisindex);
	send_byte('=');
	break;
      case 'I':						// Set Speed
	AXIS(axisindex).wormperiod=get_u24();
	if (GETMOTORPROPERTY(AXIS(axisindex).status, RUNNING)) compute_timer(axisindex);
	send_byte('='); 
	break;
      case 'G':						// Set Mode/Direction
	if ((cmd[2] != '1') && (cmd[2] != '2')) goto cant_do;
	if (cmd[2] == '1') {
	  unsigned char modedir=get_u8(); 
	  if (modedir&0x0F) SETMOTORPROPERTY(ra.status, BACKWARD); else UNSETMOTORPROPERTY(ra.status, BACKWARD);
	  modedir=(modedir>>4); 
	  if (modedir==0) { UNSETMOTORPROPERTY(ra.status, SLEWMODE); SETMOTORPROPERTY(ra.status, HIGHSPEED);}
	  else if (modedir==1) { SETMOTORPROPERTY(ra.status, SLEWMODE); UNSETMOTORPROPERTY(ra.status, HIGHSPEED); }
	  else if (modedir==2) { UNSETMOTORPROPERTY(ra.status, SLEWMODE); UNSETMOTORPROPERTY(ra.status, HIGHSPEED);}
	  else if (modedir==3) { SETMOTORPROPERTY(ra.status, SLEWMODE); SETMOTORPROPERTY(ra.status, HIGHSPEED); }
	}
	if (cmd[2] == '2') {
	  unsigned char modedir=get_u8(); 
	  if (modedir&0x0F) SETMOTORPROPERTY(de.status, BACKWARD); else UNSETMOTORPROPERTY(de.status, BACKWARD);
	  modedir=(modedir>>4); 
	  if (modedir==0) { UNSETMOTORPROPERTY(de.status, SLEWMODE); SETMOTORPROPERTY(de.status, HIGHSPEED);}
	  else if (modedir==1) { SETMOTORPROPERTY(de.status, SLEWMODE); UNSETMOTORPROPERTY(de.status, HIGHSPEED); }
	  else if (modedir==2) { UNSETMOTORPROPERTY(de.status, SLEWMODE); UNSETMOTORPROPERTY(de.status, HIGHSPEED);}
	  else if (modedir==3) { SETMOTORPROPERTY(de.status, SLEWMODE); SETMOTORPROPERTY(de.status, HIGHSPEED); }
	 }
	 send_byte('='); 
	break;
      case 'H':                                          	// Set Goto Target
	AXIS(axisindex).target=get_u24();
	send_byte('=');
	break;
      case 'M':                                          	// Set Goto BreakSteps
	AXIS(axisindex).target_slow=get_u24();
	send_byte('=');
	break;
      case 'U':                                          	// Set BreakSteps
	AXIS(axisindex).breaks=get_u24();
	send_byte('=');
	break;
      case 'd':
	RAENABLE=0; DEENABLE=0;
	send_byte('=');
	break;
      default: goto cant_do;
      }
      goto next_cmd;
    cant_do: send_byte('!'); // Can't execute command 
    next_cmd: send_byte('\x0d'); usart_index=0,usart_rdy=0;
    } // Usart received 
  }
}
